//--------------------------------------------------------------------------------------//
//                                                                                      //
//    __  __  __    __    __  ___   __   ___  __    ___    __  __  __  ___  ___  __     //
//   / _)/  \(  )  (  )  (  )(   \ (  ) (  ,)(  )  (  _)  (  \/  )/  \(   \(  _)(  )    //
//  ( (_( () ))(__  )(__  )(  ) ) )/__\  ) ,\ )(__  ) _)   )    (( () )) ) )) _) )(__   //
//   \__)\__/(____)(____)(__)(___/(_)(_)(___/(____)(___)  (_/\/\_)\__/(___/(___)(____)  //
//                                                                                      //
//                                                                                      //
//       Copyright by Theodor Mader, 2009                                               //
//			www.theomader.com/public/Projects.html                                      //
//--------------------------------------------------------------------------------------//

#include <vector>
#include <algorithm>
#include <cassert>
#include <fstream>

#include "Geometry.h"

namespace Geometry 
{
	/*!	Calculates the covariance matrix of the x, y and z coordinates of \a vertices.	*/
	void CovarianceMatrix( const std::vector<unsigned int>& vertexIndices, const std::vector<Vertex>& vertices, double result[3][3], Vertex& mean ) 
	{
		// initialize temporary variables
		double x = 0;
		double y = 0;
		double z = 0;

		double xx = 0.0;
		double yy = 0.0;
		double zz = 0.0;

		double xy = 0.0;
		double xz = 0.0;
		double yz = 0.0;

		// compute sums of all components and products (x, y, z, x*x, y*y, z*z, x*y, x*z, y*z)
		size_t numVertices = vertexIndices.size();
		for( size_t i=0; i<numVertices; i++ ) 
		{
			Vertex v = vertices[vertexIndices[i]];
			double vx = static_cast<double>( v.x );
			double vy = static_cast<double>( v.y );
			double vz = static_cast<double>( v.z );

			x += vx;
			xx += vx * vx;

			y += vy;
			yy += vy * vy;

			z += vz;
			zz += vz * vz;

			xy += vx * vy;
			xz += vx * vz;
			yz += vy * vz;
		}

		// normalize
		double n = static_cast<double>( numVertices );
		double nn = 1.0 / (n*n);

		xx /= n;
		yy /= n;
		zz /= n;

		xy /= n;
		xz /= n;
		yz /= n;

		// compute upper triangle of covariance matrix (rest is symmetric)
		result[0][0] = xx - nn * (x * x);
		result[0][1] = xy - nn * (x * y);
		result[0][2] = xz - nn * (x * z);

		result[1][1] = yy - nn * (y * y);
		result[1][2] = yz - nn * (y * z);

		result[2][2] = zz - nn * (z * z);

		// assign lower triangle now
		result[1][0] = result[0][1];
		result[2][0] = result[0][2];
		result[2][1] = result[1][2];

		mean.x = static_cast<float>( x / n );
		mean.y = static_cast<float>( y / n );
		mean.z = static_cast<float>( z / n );
	}

	/*! Calculates the covariance matrix of all vertices indexed by \a vertexIndices, uses JAMA
	*	(part of http://math.nist.gov/tnt/) to	do an Eigenvalue decomposition of the resulting matrix.
	*	The mean of all indexed vertices is then calculated and a separating plane normal to the direction
	*	of highest variance (direction of biggest eigenvector of covariance matrix) passing through the
	*	mean point is calculated	*/
	Plane SeparatingPlane( const std::vector<unsigned int>& vertexIndices, const std::vector<Vertex>& vertices ) 
	{
		double covarianceMatrix[3][3];
		Vertex meanVertex;
		CovarianceMatrix( vertexIndices, vertices, covarianceMatrix, meanVertex );

		double x, y, z;

		ComputeBiggestEigenvector( (double*)covarianceMatrix, x, y, z );
		Vertex biggestEigenVector( static_cast<float>( x ), static_cast<float>( y ), static_cast<float>( z ) );


		//	std::set<float> projections;

		// project vertices to biggest ev
		std::vector< float > projections( vertexIndices.size() ); 
		for( unsigned int i=0; i<vertexIndices.size(); i++ ) {
			Vertex v = vertices[vertexIndices[i]] - meanVertex;
			double p = v.x * biggestEigenVector.x + v.y * biggestEigenVector.y + v.z * biggestEigenVector.z;
			projections[i] = static_cast<float>( p );
		}

		// calculate median projection
		std::sort( projections.begin(), projections.end() );

		float medianProj;
		if( projections.size() % 2 ) 
			medianProj = projections[projections.size() / 2];
		else
			medianProj = (projections[projections.size()/2] + projections[projections.size()/2 - 1]) / 2.0f;

		// vertex on plane
		Vertex pointOnPlane = meanVertex + biggestEigenVector * static_cast<float>( medianProj );

		// median lies on separating plane, calculate plane offset D
		float d = biggestEigenVector.x*pointOnPlane.x + biggestEigenVector.y * pointOnPlane.y 
			+ biggestEigenVector.z*pointOnPlane.z;

		return Plane( biggestEigenVector.x, biggestEigenVector.y, biggestEigenVector.z, -d );
	}


	Vertex Vertex::operator+( const Vertex& other ) const 
	{
		return Vertex( x+other.x, y+other.y, z+other.z );
	}

	Vertex Vertex::operator-( const Vertex& other ) const 
	{
		return Vertex( x-other.x, y-other.y, z-other.z );
	}

	Vertex Vertex::operator*( float scale ) const 
	{
		return Vertex( x*scale, y*scale, z*scale );
	}

	Vertex& Vertex::operator+=( const Vertex& other ) 
	{
		x += other.x;
		y += other.y;
		z += other.z;
		return *this;
	}

	Vertex& Vertex::operator/=( const float c ) 
	{
		x /= c;
		y /= c;
		z /= c;
		return *this;
	}

	bool Vertex::operator<( const Vertex& other ) const
	{
		if( x < other.x ) 
			return true;
		else if( x == other.x )
		{
			if( y < other.y )
				return true;
			else if( y == other.y )
			{
				if( z < other.z )
					return true;
				else 
					return false;
			}
			else 
				return false;
		}
		else
			return false;
	}


	bool Plane::Valid() const 
	{
		return !( x==0.0f && y==0.0f && z==0.0f && d==0.0f );
	}

	unsigned int Plane::RequiredBufferSize() const
	{
		if( Valid() )
			return sizeof( char ) + 4 * sizeof( float );
		else
			return sizeof( char );
	}

	unsigned int Plane::WriteToBuffer( char* buffer, unsigned int pos, unsigned int bufferLength ) const 
	{
		assert( pos + RequiredBufferSize() <= bufferLength );

		// write single byte to determine plane validity
		char isValid = Valid();

		memcpy( buffer + pos, &isValid, sizeof( char ) );
		pos += sizeof( char );

		// write plane parameters (if valid)
		if( isValid ) 
		{
			memcpy( buffer + pos, &x, sizeof( float ) ); pos += sizeof( float );
			memcpy( buffer + pos, &y, sizeof( float ) ); pos += sizeof( float );
			memcpy( buffer + pos, &z, sizeof( float ) ); pos += sizeof( float );
			memcpy( buffer + pos, &d, sizeof( float ) ); pos += sizeof( float );
		}

		return pos;
	}

	void Plane::WriteToStream( std::ostream& stream ) const 
	{
		assert( stream.good() );

		// write single byte to determine plane validity
		char isValid = Valid();
		stream.write( (char*)&isValid, sizeof( char ) );

		// write plane parameters (if valid)
		if( isValid ) 
		{
			stream.write( (char*)&x, sizeof( float ) );
			stream.write( (char*)&y, sizeof( float ) );
			stream.write( (char*)&z, sizeof( float ) );
			stream.write( (char*)&d, sizeof( float ) );
		}
	}
}